import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration
from launch.conditions import IfCondition


def generate_launch_description():
    # 获取功能包路径
    pkg_share = get_package_share_directory('fruit_cartographer')
    
    # 启动参数配置
    use_sim_time = LaunchConfiguration('use_sim_time', default='false')
    resolution = LaunchConfiguration('resolution', default='0.05')
    publish_period_sec = LaunchConfiguration('publish_period_sec', default='1.0')
    
    # 配置文件路径
    cartographer_config_dir = LaunchConfiguration('cartographer_config_dir', 
                                                  default=os.path.join(pkg_share, 'config'))
    configuration_basename = LaunchConfiguration('configuration_basename', 
                                                 default='fruitbot_2d_localization.lua')
    
    # 地图文件路径（使用源代码目录中的地图文件）
    default_map_path = '/home/mechax/zkw/fruit_ws/src/fruit_cartographer/map/realmap1.pbstream'
    map_filename = LaunchConfiguration('map_filename', default=default_map_path)
    
    # RViz配置文件路径
    rviz_config_dir = os.path.join(pkg_share, 'config', 'cartographer.rviz')
    
    # 其他参数
    start_rviz = LaunchConfiguration('start_rviz', default='true')
    scan_topic = LaunchConfiguration('scan_topic', default='/scan')
    imu_topic = LaunchConfiguration('imu_topic', default='/imu')
    
    # 检查地图文件
    if os.path.exists(default_map_path):
        print(f"找到地图文件: {default_map_path}")
    else:
        print(f"警告：默认地图文件不存在: {default_map_path}")
        print("请确保地图文件存在或通过map_filename参数指定正确路径")
    
    print("=" * 60)
    print("   Cartographer 纯定位模式启动")
    print("=" * 60)
    print(f"地图文件: {default_map_path}")
    print(f"配置文件: fruitbot_2d_localization.lua")
    print(f"使用仿真时间: false")
    print("=" * 60)

    return LaunchDescription([
        # 声明启动参数
        DeclareLaunchArgument(
        'use_sim_time',
        default_value='false',
            description='是否使用仿真时间（Gazebo环境设为true，真实机器人设为false）'),
        
        DeclareLaunchArgument(
            'cartographer_config_dir',
            default_value=os.path.join(pkg_share, 'config'),
            description='配置文件目录的完整路径'),
        
        DeclareLaunchArgument(
            'configuration_basename',
            default_value='fruitbot_2d_localization.lua',
            description='Cartographer配置文件名称'),
        
        DeclareLaunchArgument(
        'map_filename',
        default_value=default_map_path,
            description='地图文件(.pbstream)的完整路径'),
        
        DeclareLaunchArgument(
            'resolution',
            default_value='0.05',
            description='栅格地图分辨率'),
        
        DeclareLaunchArgument(
            'publish_period_sec',
            default_value='1.0',
            description='地图发布周期（秒）'),
        
        DeclareLaunchArgument(
        'start_rviz',
        default_value='true',
            description='是否启动RViz可视化'),

        DeclareLaunchArgument(
        'scan_topic',
        default_value='/scan',
            description='激光雷达数据话题名称'),

        DeclareLaunchArgument(
        'imu_topic',
        default_value='/imu',
            description='IMU数据话题名称'),

        # Cartographer纯定位节点
        Node(
        package='cartographer_ros',
        executable='cartographer_node',
        name='cartographer_node',
        output='screen',
        parameters=[{'use_sim_time': use_sim_time}],
            arguments=[
                '-configuration_directory', cartographer_config_dir,
                   '-configuration_basename', configuration_basename,
                '-load_state_filename', map_filename,
                '-load_frozen_state', 'true'
            ],
        remappings=[
                ('scan', scan_topic),
                ('imu', imu_topic),
            ]),

        # 地图发布节点
        Node(
        package='cartographer_ros',
        executable='cartographer_occupancy_grid_node',
        name='cartographer_occupancy_grid_node',
        output='screen',
        parameters=[{'use_sim_time': use_sim_time}],
            arguments=[
                '-resolution', resolution,
                '-publish_period_sec', publish_period_sec
            ]),

        # RViz可视化节点
        Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        arguments=['-d', rviz_config_dir],
        parameters=[{'use_sim_time': use_sim_time}],
        output='screen',
            condition=IfCondition(start_rviz)),
    ]) 